#define  _CRT_SECURE_NO_WARNINGS
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>

#ifdef NIDAQmxBase
#include "NIDAQmxBase.h"
#else
#include "NIDAQmx.h"
#endif

#include <cmath>
#include <iostream>
#include "daqAdapter.h"

#ifdef _WIN32
	#include <Windows.h>
#else
	#include <sys/time.h>
	#include <sys/resource.h>
#endif
const double pi = 3.141592653589793;
//------------------------------------------------------------------------------------------
// check result of NIDAQ function call, exit if error
void check(int32 res, const char* str)
{
	if( res )
	{
		fprintf(stderr,"NIDAQ result %d in %s\n", res, str);
		if( res<0 ){
			char errBuf[2048];
			DAQmxGetExtendedErrorInfo(errBuf,sizeof(errBuf));
			fprintf(stderr,errBuf);
			exit(1);
		}
	}
}

int DaqIO::taskCount=0;

DaqIO::DaqIO(const char* chSpec, const int rate, const double minVal, const double maxVal, const bool isReadBlocking)	
{
#ifdef _WIN32
	SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);
#else
	setpriority(PRIO_PROCESS, 0, -1);
#endif
	readBlocking = isReadBlocking;
	char taskName[256];
	//sprintf(taskName, "Task-%s-%dhz-%f-%f", chSpec,rate, minVal, maxVal);
	sprintf(taskName,"Task-%d",taskCount++);
	//std::cout << "Creating Task : "<< taskName << ":" << chSpec << "Blocking:" << readBlocking<< std::endl;
	check(DAQmxCreateTask(taskName, &task),taskName);

	if(strstr(chSpec,"ai"))
	{
		taskType = AI;
		//create channel
		check(DAQmxCreateAIVoltageChan(task,chSpec,NULL,DAQmx_Val_RSE, minVal, maxVal,DAQmx_Val_Volts, NULL), chSpec);
		//set buffer and timing
		check(DAQmxCfgSampClkTiming(task, NULL, rate*inAverage, DAQmx_Val_Rising, DAQmx_Val_ContSamps, (uInt64)(inBufDur*rate*inAverage)), "timing/buffer input");
		check(DAQmxSetReadOverWrite(task, DAQmx_Val_OverwriteUnreadSamps), "overwrite mode");
		if(readBlocking)
		{
			// first input task is blocking
			check(DAQmxSetReadRelativeTo(task, DAQmx_Val_CurrReadPos ), "current read pos");
			check(DAQmxSetReadOffset(task, 0 ), "offset: 0" ); 
		}
		else
		{
			// remaining input Task return latest data
			check(DAQmxSetReadRelativeTo(task, DAQmx_Val_MostRecentSamp ), "most recent sample");
			check(DAQmxSetReadOffset(task, -inAverage ), "offset: -average" ); 
		}


		check(DAQmxGetTaskNumChans(task, &numChan),"GetTaskNumChan");
		buffer = new double[numChan*inAverage];
	}
	else if(strstr(chSpec,"ao"))
	{
		taskType = AO;
		check(DAQmxCreateAOVoltageChan(task, chSpec, NULL,	minVal, maxVal, DAQmx_Val_Volts, NULL), chSpec);
		check(DAQmxCfgOutputBuffer(task, 0), "buffer output");
		check(DAQmxGetTaskNumChans(task, &numChan),"GetTaskNumChan");
		buffer = new double[numChan];
	}
	else if(strstr(chSpec,"port"))
	{
		taskType = DO;
		check(DAQmxCreateDOChan(task, chSpec, NULL,DAQmx_Val_ChanPerLine), "Create P1.0"); 
		numChan=1;
	}
	else if(strstr(chSpec,"ctr"))
	{
		taskType = CTR;
		check(DAQmxCreateCIAngEncoderChan(task, chSpec, NULL,DAQmx_Val_X1,false,NULL,DAQmx_Val_ALowBLow ,DAQmx_Val_Ticks ,16*128,0,NULL), "Create ctr0"); //
		numChan=1;
	}
	else
	{
		std::cerr << "invalid channel specifier" << std::endl;
		assert(0); //should not happen
	}

	check(DAQmxStartTask(task),"start task");

}

DaqIO::~DaqIO()
{
	if(task)
	{
		check(DAQmxStopTask(task),"stop task");
		check(DAQmxClearTask(task),"clear task");
	}
	if(buffer)
	{
		delete[] buffer;
	}
	//std::cerr << "Task Destroyed" << std::endl;
}

int DaqIO::read(double * data)
{

	switch (taskType)
	{

	 	case AI: 
			int32 numRead;

			if (readBlocking)
			{	
				uInt32 numAv;
				check(DAQmxGetReadAvailSampPerChan(task, &numAv), "number available");
				//std::cout << "Data in driver buffer" << numAv << std::endl;
				//check if we're falling behind
				if(numAv>inAverage ||
					DAQmxReadAnalogF64(task, inAverage, 1, 	DAQmx_Val_GroupByScanNumber,
					buffer, numChan*inAverage, &numRead, NULL)==DAQmxErrorSamplesNoLongerAvailable)
				{
					//we're falling behind the clock, reset cursor to read the most recent samples
//#ifndef NDEBUG
					fprintf(stderr, "falling behind %f%%\n", 100.*(numAv-inAverage)/inAverage);
//#endif
					check(DAQmxSetReadRelativeTo(task, DAQmx_Val_MostRecentSamp ),	"most recent sample");
					check(DAQmxSetReadOffset(task, -inAverage ), "offset: -average" ); 
					check(DAQmxReadAnalogF64(task, inAverage, 1, DAQmx_Val_GroupByScanNumber, 
						buffer, numChan*inAverage, &numRead, NULL), "read data");
					//restore cursor to current
					check(DAQmxSetReadRelativeTo(task, DAQmx_Val_CurrReadPos ), "current read pos");
					check(DAQmxSetReadOffset(task, 0 ), "offset: 0" );
				}
			}
			else
			{
				// remaining Task: read most recent
				check(DAQmxReadAnalogF64(task, inAverage, 1, DAQmx_Val_GroupByScanNumber, buffer, numChan*inAverage, &numRead, NULL)," read remaining data");
			}

			//average, copy to caller-allocated data
			for(unsigned int i=0;i<numChan;i++){
				data[i]=0;
				for(int j=0;j<inAverage;j++){
					data[i] += buffer[numChan*j+i];
				}
				data[i]/= inAverage;
			}

			return numRead;
		case CTR:
			uInt32 ctrVal;
			check(DAQmxReadCounterScalarU32(task, 10, &ctrVal,NULL),"Read counter");
			data[0] =(double)ctrVal;
			return 1;
		default:
			fprintf(stderr, "Mismatch channel type in read():%d\n", taskType);
			assert(0);
			return -1;
	}
}

int DaqIO::write(const double *data)
{
	switch(taskType)
	{
		case AO:
			check(DAQmxWriteAnalogF64(task, 1, 0, 0, DAQmx_Val_GroupByChannel, (float64*)data, NULL, NULL), "write data");
			break;
		case DO:
			uInt8 doutBuffer[1]={(uInt8)data[0]};
			check(DAQmxWriteDigitalLines(task,1,TRUE,0,DAQmx_Val_GroupByChannel, doutBuffer, NULL,NULL), "digit out");
			break;

	}
	return 0;
}


DaqGroupIO::DaqGroupIO(
	const std::vector<std::string> inChanGroup, const int rate, 
	const double minVal, const double maxVal)
{
	numChan=0;
	for(unsigned int i=1; i< inChanGroup.size(); i++){
		daqGroup.push_back(new DaqIO(inChanGroup[i].c_str(), rate, minVal, maxVal, false));
		numChan+= daqGroup.back()->numChan;
	}
	//start the blocking task late, allowing nonblocking ones' buffer to fill up.
	daqGroup.insert(daqGroup.begin(), new DaqIO(inChanGroup[0].c_str(), rate, minVal, maxVal, true));
	numChan+= daqGroup.front()->numChan;
}

int DaqGroupIO::read(double *data)
{
	double *ptr=data;
	for(unsigned int i=0;i<daqGroup.size();i++){

		daqGroup[i]->read(ptr);
		ptr += daqGroup[i]->numChan;
		
	}
	return 0;
}

int DaqGroupIO::write(const double * data)
{
	const double *ptr=data;
	for(unsigned int i=0;i<daqGroup.size();i++){
		daqGroup[i]->write(ptr);
		ptr += daqGroup[i]->numChan;
	}return 0;
}


DaqGroupIO::~DaqGroupIO()
{
	for(unsigned int i=0;i<daqGroup.size();i++)
		delete daqGroup[i];

}


#if 0

//------------------------------------------------------------------------------------------
// remap output

double remap(double x, int ch, int gr)
{
	// get preceding position
	int i = (int) floor((x-3)/0.1);

	// return if outside (3,7)
	if( i<0 || i>39 )
		return x;

	// linear interpolation
	int adr = ch + 32*gr;
	double v = remapTable[i][adr] + 
		(remapTable[i+1][adr]-remapTable[i][adr]) * (x - (3+i*0.1)) / 0.1;

	if( v<3 || v>7 )
		mexPrintf("ch=%d, gr=%d, adr=%d, x=%f:  v=%f\n", ch, gr, adr, x, v); 
//	mexPrintf("%lf  =>  %lf\n", x, v);

	// remove bad calibrations
	if( adr==20 || adr==21 || adr==34 || adr==35 )
		v = x;

	return v;
}





//------------------------------------------------------------------------------------------
// convert raw sensor data to physical quantities
/* 
void kkConvert(double* dst, double* src)
{
	for( int i=0; i<4; i++ )
		dst[i] = 1 + 6*(src[i]-calps[i][0])/(calps[i][1]-calps[i][0]);

	for( int i=0; i<2; i++ )
		dst[4+i] = pi/2*(src[4+i]-calpot[i][0])/(calpot[i][1]-calpot[i][0]);
}
*/


//------------------------------------------------------------------------------------------
// initialize communications
//mxArray* dsInit(const int nin)
DaqIO::DaqIO(const std::vector<ChannelGroup> inChGroup, const double inMin, const double inMax, 
		    const std::vector<ChannelGroup> outChGroup,const double outMin, const double outMax,
			char* remapFile)
{
	char str[256];

	int inChNum =0,	outChNum=0;
	//create input Task
	inTask = new TaskHandle[inChGroup.size()];
	for(int i=0;i<inChGroup.size();i++){
		inChNum += inChGroup[i].chEnd-inChGroup[i].chStart;
		sprintf(str, "input %d", i+1)
		check(DAQmxCreateTask(inChGroup[i].deviceName, inTask[i]), str);

		for(j=inChGroup[i].chStart; j<inChGroup[i].chEnd; j++)
		{
			sprintf(str, "%s/ai%d",inChGroup[i].deviceName, j);
			check(DAQmxCreateAIVoltageChan(inTask[i], str, NULL, DAQmx_Val_RSE, 
				inMin, inMax, DAQmx_Val_Volts, NULL), str);
		}
	

		// set buffers and timing
		check(DAQmxCfgSampClkTiming(inTask[i], NULL, inRate, DAQmx_Val_Rising, 
			DAQmx_Val_ContSamps, (uInt64)(inBufDur*inRate)), "timing/buffer input");
		check(DAQmxSetReadOverWrite(inTask[i], DAQmx_Val_OverwriteUnreadSamps), 
			"overwrite mode");
		if( i==0 )
		{
			// first input task is blocking
			check(DAQmxSetReadRelativeTo(inTask[i], DAQmx_Val_CurrReadPos ), "current read pos");
			check(DAQmxSetReadOffset(inTask[i], 0 ), "offset: 0" ); 
		}
		else
		{
			// remaining input Task return latest data
			check(DAQmxSetReadRelativeTo(inTask[i], DAQmx_Val_MostRecentSamp ), "most recent sample");
			check(DAQmxSetReadOffset(inTask[i], -inAverage ), "offset: -average" ); 
		}

		// start task
		check(DAQmxStartTask(inTask[i]), "start input");
	} //end-of-input-task creation

	// create output Task
	for(int i=0; i<outChGroup[i].size(); i++ )
	{
		outChNum += outChGroup[i].chEnd-outChGroup[i].chStart;
		// create task
		sprintf(str, "output %d", i+1);
		check(DAQmxCreateTask(str, outTask+i), str);

		// add output channels
		for(int j=outChGroup[i].chStart; j<outChGroup[i].chEnd; j++ ) 
		{
			sprintf(str, "%s/ao%d", outChGroup[i].deviceName, j);
			check(DAQmxCreateAOVoltageChan(outTask[i], str, NULL,
				outMin, outMax, DAQmx_Val_Volts, NULL), str);
		}

		// set buffers
		check(DAQmxCfgOutputBuffer(outTask[i], 0), "buffer output");

		// start task
		check(DAQmxStartTask(outTask[i]), "start output");
	} //end-of-outtask-creation
	
	inBuffer = new double[inChNum*inAverage];
	outBuffer= new double[outChNum];


	if (remapFile){
		loadRemapData(remapFile);
	}
	/*
	// start timer and pause
	timeBeginPeriod(1);
	Sleep(100);
	tmlast = timeGetTime();
	*/
}


int DaqIO::loadRemapData(char* remapFile)
{
		assert(!remapTable);
		// output remapping
		FILE* fp;
		double buffer[4];
		fp = fopen(remapFile, "rb");
		if( fp )
		{
			fread(buffer, 4, sizeof(double), fp);
			remapTable = new double[41][76];
			int nread = fread(remapTable[0], sizeof(double), 41*76, fp);
			fclose(fp);
			fprintf(stdout,"remapall.bin loaded\n");
			if( nread != 41*76 )
				fprintf(stderr,"wrong size of remapall.bin");
		}
		else{
			fprintf(stderr,"remapall.bin not found, proceeding with raw outputs\n");
		}
}


//---------------------------------------------------------------------------------------
// read data from all input groups

int DaqIO::readChannels(mxArray* pout[], const int nin)
{
	double status;
	uInt32 numav;
	int i, j, k;
	double *ptr;

	// create Matlab matrices
	for( i=0; i<inGroups; i++ )
		pout[i] = mxCreateDoubleMatrix(1, inChanNum[i], mxREAL);

	// read Task, reset task 0 if necessary
	for( i=0; i<inGroups; i++ )
	{
		// task 0: blocking, reset if call is too late
		if( i==0 )
		{
			check(DAQmxGetReadAvailSampPerChan(inTask[0], &numav), "number available");
			if( numav>inAverage ||
				DAQmxReadAnalogF64(inTask[0], inAverage, 1, 
				DAQmx_Val_GroupByScanNumber, inBuffer, inChanMax*inAverage, NULL, NULL) ==
				DAQmxErrorSamplesNoLongerAvailable )
			{
				check(DAQmxSetReadRelativeTo(inTask[0], DAQmx_Val_MostRecentSamp ), 
					"most recent sample");
				check(DAQmxSetReadOffset(inTask[0], -inAverage ), "offset: -average" ); 
				check(DAQmxReadAnalogF64(inTask[0], inAverage, 1, 
					DAQmx_Val_GroupByScanNumber, inBuffer, inChanMax*inAverage, NULL, NULL),
					"read data");
				check(DAQmxSetReadRelativeTo(inTask[0], DAQmx_Val_CurrReadPos ), 
					"current read pos");
				check(DAQmxSetReadOffset(inTask[0], 0 ), "offset: 0" );

				status = numav;
				status = -status-1;

				if( nin>1 )
					mexPrintf("missed sample, %d\n", numav);
			}
			else
				status = numav;
		}

		// remaining Task: read most recent
		else
			check(DAQmxReadAnalogF64(inTask[i], inAverage, 1, 
			DAQmx_Val_GroupByScanNumber, inBuffer, inChanMax*inAverage, NULL, NULL),
			" read remaining data");


		// average data, copy to Matlab
		ptr = mxGetPr(pout[i]);
		for( j=0; j<inChanNum[i]; j++ )
		{
			ptr[j] = 0;
			for( k=0; k<inAverage; k++ )
				ptr[j] += inBuffer[inChanNum[i]*k + j];
			ptr[j] /= inAverage;
		}
	}

	// convert ???

	// report status and timing
	pout[inGroups] = mxCreateDoubleScalar(status);
	pout[inGroups+1] = mxCreateDoubleScalar(timeGetTime()-tmlast);
	tmlast = timeGetTime();
}


//------------------------------------------------------------------------------------------
// write data to specified output groups

void dsWrite(const int nin, const mxArray* pin[])
{
	int i, j, idx;
	double* ptr;

	if( nin%2 )
		mexErrMsgTxt("'write' expects even number of arguments");

	// loop over output groups
	for( i=0; i<nin; i+=2 )
	{
		// get group index
		mxcheck(pin[i], 1);
		idx = (int)mxGetScalar(pin[i]) - 1;
		if( idx<0 || idx>=outGroups )
			mexErrMsgTxt("group index out of range");

		// get group data, remap
		mxcheck(pin[i+1], outChanNum[idx]);
		ptr = mxGetPr(pin[i+1]);

		for( j=0; j<outChanNum[idx]; j++ )
			outBuffer[j] = ptr[j];

		// remap, group 0 only
		if( flgRemap )
			for( j=0; j<outChanNum[idx]; j++ )
				outBuffer[j] = remap(ptr[j], j, idx);
		else
			for( j=0; j<outChanNum[idx]; j++ )
				outBuffer[j] = ptr[j];

		// write data
		check(DAQmxWriteAnalogF64(outTask[idx], 1, 0, 0, DAQmx_Val_GroupByChannel, 
			outBuffer, NULL, NULL), "write data");
	}
}


//-----------------------------------------------------------------------------------------
// function called at mex exit

void DaqIO::~DaqIO(void)
{
	// stop and clear Task
	if( initialized )
	{
		for( i=0; i<inGroups; i++ )
		{
			check(DAQmxStopTask(inTask[i]), "stop input");
			check(DAQmxClearTask(inTask[i]), "clear input");
		}

		for( i=0; i<outGroups; i++ )
		{
			check(DAQmxStopTask(outTask[i]), "stop output");
			check(DAQmxClearTask(outTask[i]), "clear output");
		}

		initialized = false;
		timeEndPeriod(1);
	}
}



#endif
